
import os
import pybullet as p
import pybullet_data as pd
import math
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())  # 设置pybullet_data的文件路径
# flags = p.URDF_INITIALIZE_SAT_FEATURES#0#p.URDF_USE_SELF_COLLISION

pandaUid=p.loadURDF("urdf6/urdf/urdf6.urdf",useFixedBase=True)
p.resetBasePositionAndOrientation(pandaUid, [0, 0, 0], [0, 0, 0, 1])
joint_num=p.getNumJoints(pandaUid)

#滑动条控制运动
paramIds = []
for i in range(joint_num):
    jointName='joint'+str(i)
    paramIds.append(p.addUserDebugParameter(jointName, -3.14, 3.14, 0))

while True:
    p.stepSimulation()
    for i in range(len(paramIds)):
        c = paramIds[i]
        targetPos = p.readUserDebugParameter(c)
        p.setJointMotorControl2(pandaUid,int(i), p.POSITION_CONTROL, targetPos, force=5 * 240.)





# # 模拟连续运动
# for i in range(joint_num):
#     p.setJointMotorControl2(pandaUid, i, p.POSITION_CONTROL, targetPosition=0, force=5 * 240.)
    
# target_pos=[-math.pi/2,-1.587,-1.025,1.421,0,0,0]
# p.setJointMotorControlArray(pandaUid, [0,1,2,3,4,5,6], p.POSITION_CONTROL, target_pos)
# p.stepSimulation()

# timeStep=1./60.
# p.setTimeStep(timeStep)
# t=0
# time.sleep(1)
# # 开始记录
# # logging_id = p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "review.mp4")
# step0=1*math.pi/180
# step67=1*math.pi/180
# while True:
#     p.stepSimulation()
#     target_pos[0]+=step0
#     target_pos[6]+=step67
#     target_pos[5]+=step67
    
#     p.setJointMotorControlArray(pandaUid, [0,1,2,3,4,5,6], p.POSITION_CONTROL, target_pos)
#     if target_pos[0]-math.pi/2>0.01:
#         step0=-step0
#     if target_pos[0]+math.pi/2<-0.01:
#         step0=-step0
        
#     if target_pos[6]-0.5>0.01:
#         step67=-step67
#     if target_pos[6]<-0.01:
#         step67=-step67
#     time.sleep(0.01)
    
    
# time.sleep(1)
# # 停止记录
# # p.stopStateLogging(logging_id)
while True:
    pass
